#include "arm_robot_hardware_drivers/ScaarDriver.h"


#include <thread>
#include <chrono>
#include "common/crc_ccitt.hpp"

#define FW_VERSION "0.0.1"
#define PI 3.14159f

namespace arm_robot_hardware_drivers {

void ScaarDriver::init(std::string port, int baudrate, int num_joints)
{
    // @TODO read version from config
    version_ = FW_VERSION;

    //set up the msg size
    inMsgSize_ = sizeof(inMsg_);
    outMsgSize_ = sizeof(outMsg_);
    memset(&outMsg_, 0, sizeof(SCA_ARM_CMD_MSG));
    outMsg_.head[0] = 0xff;
    outMsg_.head[1] = 0xfe;
    outMsg_.mode = 0x00;
    memset(&inMsg_, 0, sizeof(SCA_ARM_FBK_MSG));
    SA100angle_ = 0.0f;

    // initialise joint and encoder calibration
    num_joints_ = num_joints;
    motorstate_.resize(num_joints_);
    motorcmd_.resize(num_joints_);

    setWork_Kp_Kd();

    // establish connection with teensy board
    try
    {
        serial_.setPort(port);
        serial_.setBaudrate(baudrate);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        serial_.setTimeout(to);
        serial_.open();
    }
    catch(serial::IOException &e)
    {
        ROS_WARN("Failed to connect to serial port %s", port.c_str());
        return;
    }

    if(serial_.isOpen())
    {
        ROS_INFO("Successfully connected to serial port %s", port.c_str());
    }
    
    initialised_ = false;

    while (!initialised_)
    {
        ROS_INFO("Waiting for response from Scaar on port %s", port.c_str());
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        exchange();
    }
    ROS_INFO("Successfully initialised driver on port %s", port.c_str());

    

    // get current joint positions
    exchange();
}

ScaarDriver::ScaarDriver()
{
}

ScaarDriver::~ScaarDriver()
{
    if (serial_.isOpen()) 
    {
        serial_.close();
    }
}
void ScaarDriver::setWork_Kp_Kd()
{
    for(int i = 0; i<num_joints_; i++)
    {
        motorcmd_[i].Kp = 0.8;
        motorcmd_[i].Kd = 0.1;
    }
}

// Update between hardware interface and hardware driver
void ScaarDriver::update(std::vector<double>& pos_commands, std::vector<double>& actuator_positions)
{
    // get updated position commands
    updatePosCmd(pos_commands);


    // run the communication with board
    exchange();


    // return updated actuator_positions
    updatePosState(actuator_positions);
}

void ScaarDriver::updatePosCmd(std::vector<double>& pos_commands)
{
    // get updated position commands
    for(int i = 0; i<num_joints_; i++)
    {
        motorcmd_[i].q = pos_commands[i];
    }
    // construct update message
    motifyOutMsg();
}

void ScaarDriver::updatePosState(std::vector<double>& actuator_positions)
{
    // extract msg from inMsg_
    
    // updated actuator_positions
    for(int i = 0; i<num_joints_; i++)
    {
        actuator_positions[i] = motorstate_[i].q;
    }
}


void ScaarDriver::calibrateJoints()
{
    sendCommand();
}

void ScaarDriver::getJointPositions(std::vector<double>& actuator_positions)
{
    // get current joint positions
    exchange();
    updatePosState(actuator_positions);
}

// Send specific commands
void ScaarDriver::sendCommand()
{
    exchange();
}

// Send msg to board and collect data
void ScaarDriver::exchange()
{
    if (!transmit())
    {
        // print err
    }
    if (!receive())
    {
        // print err
    }
    // parse msg

    checkInit();
    
}

void ScaarDriver::printOutMsgInHex()
{
    // 将 outMsg_ 转换为字节数组
    const uint8_t* data = reinterpret_cast<const uint8_t*>(&outMsg_);
    std::size_t data_size = sizeof(outMsg_);  // 获取结构体大小

    // 创建一个字符串流用于拼接输出
    std::ostringstream hex_stream;

    // 遍历每一个字节，将其转换为十六进制格式
    for (std::size_t i = 0; i < data_size; ++i) {
        // 将当前字节转换为十六进制，填充为两位宽度（例如 0x01 打印为 01）
        hex_stream << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(data[i]) << " ";
    }

    // 使用 ROS_INFO 将结果打印
    ROS_INFO("outMsg_ in hex: %s", hex_stream.str().c_str());
}

void ScaarDriver::printInMsgInHex()
{
    // 将 inMsg_ 转换为字节数组
    const uint8_t* data = reinterpret_cast<const uint8_t*>(&inMsg_);
    std::size_t data_size = sizeof(inMsg_);  // 获取结构体大小

    // 创建一个字符串流用于拼接输出
    std::ostringstream hex_stream;

    // 遍历每一个字节，将其转换为十六进制格式
    for (std::size_t i = 0; i < data_size; ++i) {
        // 将当前字节转换为十六进制，填充为两位宽度（例如 0x01 打印为 01）
        hex_stream << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(data[i]) << " ";
    }

    // 使用 ROS_INFO 将结果打印
    ROS_INFO("inMsg_ in hex: %s", hex_stream.str().c_str());
}



bool ScaarDriver::transmit()
{
    if (!serial_.isOpen()) 
    {
        ROS_INFO_STREAM("Serial port is not open!");
        return false;
    }

    size_t bytes_written = serial_.write((uint8_t *)&outMsg_, sizeof(outMsg_));
    printOutMsgInHex();

    if (bytes_written == outMsgSize_) {
        ROS_INFO_STREAM("Successfully sent " << bytes_written << " bytes.");
        return true;
    }    
    else 
    {
        ROS_WARN_STREAM("Failed to send all data. Sent " << bytes_written << " bytes out of " << outMsgSize_ << " bytes.");
        return false;
    }

    
    return true;
}

bool ScaarDriver::receive()
{
    if (!serial_.isOpen()) {
        ROS_ERROR("Serial port is not open!");
        return false;
    }

    // 设置读取超时时间为 100ms
    serial::Timeout timeout = serial::Timeout::simpleTimeout(10);
    serial_.setTimeout(timeout);

    try {
        // 读取数据
        size_t bytes_read = serial_.read((uint8_t *)&inMsg_, inMsgSize_);

        // 检查是否读取了完整的消息
        if (bytes_read != inMsgSize_) {
            ROS_WARN("Incomplete message received: only %zu bytes received, expected %d bytes.", bytes_read, inMsgSize_);
            return false;
        }

        // CRC 校验接收到的消息
        unsigned short calculated_crc = CRC_CCITT::calculate(0x0000, (uint8_t*)&inMsg_, inMsgSize_ - 2);
        printInMsgInHex();
        ROS_WARN("CRC check failed: received 0x%04X, calculated 0x%04X", inMsg_.CRC16Data, calculated_crc);

        if (inMsg_.CRC16Data != calculated_crc) {
            ROS_ERROR("CRC check failed: received 0x%04X, calculated 0x%04X", inMsg_.CRC16Data, calculated_crc);
            return false;
        } else {
            extractInMsg();
            
            received_successfully_ = true;
        }

    } catch (const std::exception& e) {
        ROS_ERROR("Exception during serial read: %s", e.what());
        return false;
    }

    return received_successfully_;
}


bool ScaarDriver::extractInMsg()
{
    // extract msg from inMsg_
    // if(CRC_CCITT::calculate(0x0000, (uint8_t *)&inMsg_, inMsgSize_ - 2) != inMsg_.CRC16Data)
    // {
        ROS_INFO("the num of joints is %d", num_joints_);
        for(int i = 0; i<num_joints_; i++)
        {
            motorstate_[i].q = (float)inMsg_.joint_states[i].actual_pos / (16384 / PI);
            motorstate_[i].dq = (float)inMsg_.joint_states[i].actual_vel / 256.0f;
            motorstate_[i].tauEst = (float)inMsg_.joint_states[i].actual_tor / 256.0f;
        
            // motorstate_[i].q = 0.2;
            // motorstate_[i].dq = 0;
            // motorstate_[i].tauEst = 0;
            // ROS_INFO("Successfully received data: motor %d angle: %f",i,motorstate_[i].q);
        }

        MRC_voltage[0] = (float)inMsg_.MRC_voltage[0] / 100.0f;
        MRC_voltage[1] = (float)inMsg_.MRC_voltage[1] / 100.0f;

        SA100angle_ = (float)inMsg_.SA100_angle / 1000.0f;

        ROS_INFO("Successfully received data: SA100 angle: %f", SA100angle_);
        
        return true;
    // }
    // return false;
    
}
void ScaarDriver::motifyOutMsg()
{
    // modify outMsg_
    outMsg_.head[0] = 0xff;
    outMsg_.head[1] = 0xfe;
    if(mode_ == 1)
    {
        outMsg_.mode = 0x01;
    }
    for(int i = 0; i<num_joints_; i++)
    {
        outMsg_.joint_cmd[i].id = i;
        outMsg_.joint_cmd[i].des_pos = motorcmd_[i].q * (16384 / PI);
        outMsg_.joint_cmd[i].des_vel = motorcmd_[i].dq * 256;
        outMsg_.joint_cmd[i].des_tor = motorcmd_[i].tau * 256;
        outMsg_.joint_cmd[i].des_kp = motorcmd_[i].Kp * 1280;
        outMsg_.joint_cmd[i].des_kd = motorcmd_[i].Kd * 1280;
    }


    outMsg_.MRC_voltage[0] = MRC_voltage[0] * 100;
    outMsg_.MRC_voltage[1] = MRC_voltage[1] * 100;

    outMsg_.CRC16Data = CRC_CCITT::calculate(0x0000, (uint8_t *)&outMsg_, outMsgSize_ - 2);
}

void ScaarDriver::checkInit()
{
    int ack = inMsg_.mode;
    if (ack == 0x01)
    {
        initialised_ = true;
        mode_ = 1;//init success switch to the postion mode 
    }
    else if (ack == 0x00)
    {
        initialised_ = false;
        ROS_INFO("Wait for initialisation completed");
    }  
}


} // namespace arm_robot_hardware_drivers


//ff fe 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 02 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 81 4b
//ff fe 01 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00 00 00 00 00 00 00 00 00 00 00 00 02 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 81 4b
